/*
 * Includes
 */ 
#include <x32.h>
#include <ucos.h>


/*
 * Global Definitions
 */ 
#define PWM_PERIOD 	peripherals[PERIPHERAL_DPC1_PERIOD]
#define PWM_WIDTH 	peripherals[PERIPHERAL_DPC1_WIDTH]
#define SSD 		peripherals[PERIPHERAL_DISPLAY]
#define ENGINE_A 	peripherals[PERIPHERAL_ENGINE_A]
#define ENGINE_B  	peripherals[PERIPHERAL_ENGINE_B]
#define TIME_SLICE 	1

// uC/OS PRIORITIES
// higher number is higher priority
#define TASK_PRIORITY 		10
#define ERROR_TASK_PRIORITY 	9


//x32 ISR PRIORITIES
// lower number is higher priority
#define PRIORITY_DECODER_A 	1
#define PRIORITY_DECODER_B 	2
#define PRIORITY_BUTTONS 	5

// LEDs Definitions
#define LED7 0x80
#define LED6 0x40
#define LED5 0x20
#define LED4 0x10
#define LED3 0x08
#define LED2 0x04
#define LED1 0x02
#define LED0 0x01

// BUTTONS Definitions
#define BTN3 0x08
#define BTN2 0x04
#define BTN1 0x02
#define BTN0 0x01

/*
 Global Variables
 */
int throttle=0, count=0, old_count=0, speed=0;
int new_a=0, new_b=0, state_a=0, state_b=0;
int stack1[1024];
int stack2[256];
OS_EVENT *sem_error;
UBYTE err;

void isr_buttons() { 													// Button interupt service
	OSIntEnter();														// OSIntEnter stops OS temporarily to stop context switching	
	if (peripherals[PERIPHERAL_BUTTONS] & BTN3)	{						// Reset button, left most on board.
		SSD 		= 0x00;												// Reset screen to zeros
		PWM_WIDTH	= 0x00;												// Reset PWM to zeros
		exit();															// Exit OS to bootloader
	}  
	else if (peripherals[PERIPHERAL_BUTTONS] & BTN0) {					// Increase button, right most on board.
		if (throttle <= 0xFFFB) throttle += 4;							// If throttle is within range -> Increase throttle by 4
	}
	else if (peripherals[PERIPHERAL_BUTTONS] & BTN1) { 					// Deacrease button, right middle button on board.
		if (throttle >= 0x0004) throttle -= 4;							// If throttle is within range -> Decrease throttle by 4
	}
	OSIntExit(); 														// Restore OS context switching
}

void isr_decode() {														// Software decoder for signals a and b
	OSIntEnter();														// OSIntEnter stops OS temporarily to stop context switching
	new_a = ENGINE_A;													// Store new status
	new_b = ENGINE_B;													// Store new status
	if (new_a != state_a && new_b != state_b) OSSemPost(sem_error);		// both signals have changed: error
	else if (new_a == 1 && new_b == 1) {								// both signals are high: update up/down counter
		if 	(state_b == 0) count++; 									// b signal comes later: positive direction
		else count--; 													// a signal comes later: negative direction
	}
	state_a = new_a;													// Update state
	state_b = new_b;													// Update state	
	OSIntExit();														// Restore OS context switching
}

void per_task(void *data) {
	while(TRUE) {
		old_count = count;			 			// Get the current count for measuring the change of count
		PWM_WIDTH = throttle;					// Assign the new throttle
		OSTimeDly(TIME_SLICE); 					// 20ms; Delay the system for one timeslice
		speed = count - old_count; 				// Speed is the rate of change of count over an interval TIME_SLICE
		SSD   = (speed<<8) + (throttle>>2);		// Speed displayed on the left and throttle displayed on the right of the SSD
	}											// SSD can hold at most 16bit value.
}

void err_task(void *data) {
	while(TRUE) {
	  OSSemPend(sem_error, WAIT_FOREVER, &err); // Pend on sem_error semaphore (Wait until posted)
	  peripherals[PERIPHERAL_LEDS] |= LED7; 	// Turn on LED7
	  OSTimeDly(5); 							// wait 100ms
	  peripherals[PERIPHERAL_LEDS] &= ~LED7; 	// Turn off LED7
	  while(OSSemAccept(sem_error) > 0);		// Restore sem_error to 0
	}
}

void main() {
	// Create Semaphores
	sem_error = OSSemCreate(0);
	
	// Configure DECODE ISR service to listen to signal a change
	SET_INTERRUPT_VECTOR	(INTERRUPT_ENGINE_A, isr_decode);
	SET_INTERRUPT_PRIORITY	(INTERRUPT_ENGINE_A, PRIORITY_DECODER_A);
	ENABLE_INTERRUPT		(INTERRUPT_ENGINE_A);

	// Configure DECODE ISR service to listen to signal b change
	SET_INTERRUPT_VECTOR	(INTERRUPT_ENGINE_B, isr_decode);
	SET_INTERRUPT_PRIORITY	(INTERRUPT_ENGINE_B, PRIORITY_DECODER_B);
	ENABLE_INTERRUPT		(INTERRUPT_ENGINE_B);
	
	// Configure the BUTTONS ISR
	SET_INTERRUPT_VECTOR	(INTERRUPT_BUTTONS, isr_buttons);
	SET_INTERRUPT_PRIORITY	(INTERRUPT_BUTTONS, PRIORITY_BUTTONS);
	ENABLE_INTERRUPT		(INTERRUPT_BUTTONS);

	PWM_PERIOD = 1024;

	OSInit();

	// Lower number -> Higher priority
	OSTaskCreate(per_task, (void *)0, (void *)stack1, PERIODIC_TASK_PRIORITY); 	// Priority=10
	OSTaskCreate(err_task, (void *)0, (void *)stack2, ERROR_TASK_PRIORITY); 	// Priority=9

	OSStart(); //also enables the global interrupts
}



